// 目标：已知旋转向量定义为沿着z轴旋转45°。下面按照该定义用Eigen实现旋转向量、旋转矩阵和四元数及其之间的互相转换。
// 程序学习目标：熟悉Eigen的使用和旋转表达方式之间的转换。

#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <iostream>

int main(int argc, char **argv) {
  // 旋转向量(轴角): 沿Z轴旋转45°
  Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));
  std::cout << "旋转向量的旋转轴 = \n" << rotation_vector.axis() << "\n旋转向量角度" << rotation_vector.angle() << std::endl;
  std::cout << std::endl;

  // 旋转矩阵：沿Z轴旋转45°
  Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
  // auto rotation_matrix = Eigen::Matrix3d::Identity();
  rotation_matrix << 0.707, -0.707, 0,
                     0.707,  0.707, 0,
                         0,      0, 1;
  std::cout << " 旋转矩阵 =\n" << rotation_matrix << std::endl;
  std::cout << std::endl;

  // 四元数：沿z轴旋转45°
  Eigen::Quaterniond quat = Eigen::Quaterniond(0, 0, 0.383, 0.924);
  std::cout << "四元数输出方法 1: 四元数=\n" << quat.coeffs() << std::endl;
  // 请注意coeffs的顺序是(x,y,z,w), w为实部，其他为虚部
  std::cout << "四元数输出方法 2: 四元数=\n ×=" << quat.x() << "\n y=" << quat.y() << "\n z=" << quat.z() << "\n w=" << quat.w() << std::endl;
  std::cout << std::endl;

  // 1.旋转矩阵转换为其他形式
  // 注意：fromRotationMatrix参数只适用于将旋转矩阵转换为旋转向量，不适用于将旋转矩阵转换为四元数
  rotation_vector.fromRotationMatrix(rotation_matrix);
  std::cout << "旋转矩阵转换为旋转向量方法 1: 能转轴 =\n" << rotation_vector.axis() << "\n旋转角度 = " << rotation_vector.angle() << std::endl;
  std::cout << std::endl;

  rotation_vector = rotation_matrix;
  std::cout << "旋转矩阵转换为旋转向量方法 2: 旋转轴 =\n" << rotation_vector.axis() << "\n旋转角度=" << rotation_vector.angle() << std::endl;
  std::cout << std::endl;

  rotation_vector = Eigen::AngleAxisd(rotation_matrix);
  std::cout << "屣转矩阵转换为旋转向量方法 3: 旋转轴 =\n" << rotation_vector.axis() << "\n旋转角度=" << rotation_vector.angle() << std::endl;
  std::cout << std::endl;

  quat = Eigen::Quaterniond(rotation_matrix);
  std::cout << "旋转矩阵转换为四元数方法 1: Q =\n" << quat.coeffs() << std::endl;
  std::cout << std::endl;

  // 2. 旋转向量转换为其他形式
  std::cout << "旋转向量转换为旋转矩阵方法 1: 旋转矩阵 R =\n" << rotation_vector.matrix() << std::endl;
  std::cout << "旋转向量转换为旋转矩阵方法 2: 旋转矩阵 R =\n" << rotation_vector.toRotationMatrix() << std::endl;
  std::cout << std::endl;

  quat = Eigen::Quaterniond(rotation_vector);
  // 请注意coeffs的顺序是(x,y,z,w), 是虚部，x,y,z是实部
  std::cout << "旋转向量转换为四元数：Q =\n" << quat.coeffs() << std::endl;
  std::cout << std::endl;

  // 3. 四元数转换为其他形式
  rotation_vector = quat;
  std::cout << "四元数转换为旋转向量：旋转轴=\n" << rotation_vector.axis() << "\n旋转角度=" << rotation_vector.angle() << std::endl;
  std::cout << std::endl;

  rotation_matrix = quat.matrix();
  std::cout << "四元数转换为旋转矩阵方法 1: 旋转矩阵=\n" << rotation_matrix << std::endl;
  std::cout << std::endl;

  rotation_matrix = quat.toRotationMatrix();
  std::cout << "四元数转换为旋转矩阵方法 2: 旋转矩阵=\n" <<rotation_matrix << std::endl;
  std::cout << std::endl;
  
  return 0;
}